#!/usr/bin/env python
from __future__ import print_function

import os
import sys
from collections import namedtuple

import numpy as np
import rospy
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import PoseStamped


def get_pose(message):
    global current_pose, current_twist
    current_pose = message.pose[-1]
    current_twist = message.twist[-1]

class master_trajectory_save():
    def __init__(self):
        file = open("/home/st/ubuntu_data/test_data/test5_ws/src/mecanum_robot_simulation/trajectory_tracking/data/master_cut2.txt", "r")
        self.tra_list = file.readlines()
        self.tra_length = len(self.tra_list)
        self.pose = PoseStamped()
   
    def get_position_at(self, t):
        index = int(t) % self.tra_length
        self.pose.header.stamp = rospy.Time.now()
        self.pose.header.frame_id = "odom"
        self.pose.pose.position.x = np.array(self.tra_list[index].strip().split(' '), dtype = np.float32  )[0]
        self.pose.pose.position.y = np.array(self.tra_list[index].strip().split(' '), dtype = np.float32  )[1]
        self.pose.pose.position.z = 0
        self.pose.pose.orientation.w = np.array(self.tra_list[index].strip().split(' '), dtype = np.float32  )[3]
        self.pose.pose.orientation.x = np.array(self.tra_list[index].strip().split(' '), dtype = np.float32  )[4]
        self.pose.pose.orientation.y = np.array(self.tra_list[index].strip().split(' '), dtype = np.float32  )[5]
        self.pose.pose.orientation.z = np.array(self.tra_list[index].strip().split(' '), dtype = np.float32  )[6]

        return self.pose

if __name__ == '__main__':
    detal_index = 50
    sim_time = 10000
    DELTA_T = 0.2
    traj = master_trajectory_save()
    rospy.init_node('control')
    # current_pose = None
    # current_twist = None
    subscriber = rospy.Subscriber('gazebo/model_states', ModelStates, get_pose)
    # twist_publisher = rospy.Publisher('computed_control_actions',
    #                                   Twist,
    #                                   queue_size=1)
    posePub = rospy.Publisher('traj_goal',
                                    PoseStamped,
                                    queue_size=1)

    # while current_pose is None or current_twist is None:
    #     pass

    STEPS = sim_time
    rate = rospy.Rate(int(1 / DELTA_T))
    t = 100
    while not rospy.is_shutdown() and t < STEPS:
        pose = traj.get_position_at(t)
        posePub.publish(pose)
        rate.sleep()
        detal_dis = (current_pose.position.x - pose.pose.position.x)**2 + (current_pose.position.y - pose.pose.position.y)**2
        if detal_dis < 0.1:
            t += detal_index

    # rospy.spin()
